/*!
 * @file MotionActuator.cpp
 * @author Rocco Martino
 */
/***************************************************************************
 *   Copyright (C) 2014 by Rocco Martino                                   *
 *   martinorocco@gmail.com                                                *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU Lesser General Public License as        *
 *   published by the Free Software Foundation; either version 2.1 of the  *
 *   License, or (at your option) any later version.                       *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this program; if not, write to the                 *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

/* ======================================================================= */
/* ....................................................................... */
#include <ooGame/MotionActuator>

#include <osgODE/Notify>
#include <osgODE/RigidBody>
#include <osgODE/World>
/* ....................................................................... */
/* ======================================================================= */




using namespace ooGame ;




/* ======================================================================= */
/* ....................................................................... */
MotionActuator::MotionActuator(MotionType motion_type):
    m_motion_type               ( motion_type ),
    m_translation_local         ( true ),
    m_rotation_local            ( true ),
    m_force_local               ( true ),
    m_torque_local              ( true ),
    m_linear_velocity_local     ( false ),
    m_angular_velocity_local    ( false ),
    m_linear_velocity_additive  ( false ),
    m_servo_limit_x             ( false ),
    m_servo_limit_y             ( false ),
    m_servo_limit_z             ( false )
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
MotionActuator::MotionActuator(const MotionActuator& other, const osg::CopyOp& copyop):
    Actuator                    ( other, copyop ),
    m_motion_type               ( other.m_motion_type ),
    m_translation               ( other.m_translation ),
    m_rotation                  ( other.m_rotation ),
    m_force                     ( other.m_force ),
    m_torque                    ( other.m_torque ),
    m_linear_velocity           ( other.m_linear_velocity ),
    m_angular_velocity          ( other.m_angular_velocity ),
    m_servo_force_min           ( other.m_servo_force_min ),
    m_servo_force_max           ( other.m_servo_force_max ),
    m_translation_local         ( other.m_translation_local ),
    m_rotation_local            ( other.m_rotation_local ),
    m_force_local               ( other.m_force_local ),
    m_torque_local              ( other.m_torque_local ),
    m_linear_velocity_local     ( other.m_linear_velocity_local ),
    m_angular_velocity_local    ( other.m_angular_velocity_local ),
    m_linear_velocity_additive  ( other.m_linear_velocity_additive ),
    m_servo_limit_x             ( other.m_servo_limit_x ),
    m_servo_limit_y             ( other.m_servo_limit_y ),
    m_servo_limit_z             ( other.m_servo_limit_z ),
    m_pid_controller            ( osg::clone(other.m_pid_controller.get(), copyop) )
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
MotionActuator::~MotionActuator(void)
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
MotionActuator::actuate( osgODE::ODEObject* object, Game* game )
{

    (void) game ;


    osgODE::RigidBody*  body = object->asRigidBody() ;

    PS_ASSERT1( body != NULL ) ;
    PS_ASSERT1( body->getWorld() != NULL ) ;




    switch( m_motion_type )
    {
        case SIMPLE_MOTION:
            doSimpleMotion( body ) ;
        break ;

        case SERVO_CONTROL:
            doServoControl( body ) ;
        break ;

        default:
            PS_BREAKPOINT() ;
        break ;
    }
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
MotionActuator::doServoControl( osgODE::RigidBody* body )
{
    PS_ASSERT1( m_pid_controller.valid() ) ;

    if( ! m_pid_controller.valid() ) {
        PS_FATAL("ooGame::MotionActuator::doServoControl(%p): null pid controller", this) ;
        return ;
    }


    osg::Vec3   axis = m_linear_velocity ;
    ooReal      speed = axis.normalize() ;

    osg::Vec3   direction = m_linear_velocity_local ? body->getQuaternion() * axis : axis ;

    osgODE::World*  world = body->getWorld() ;
    PS_ASSERT1( world != NULL ) ;

    const ooReal    err = speed - body->getLinearVelocity() * direction ;


    osgODE::PIDController*      pid = getPIDController() ;

    ooReal  force = 0.0 ;

    if( pid ) {
        force = pid->solve( err, world->getCurrentStepSize() ) ;
    } else {
        force = err ;
    }


    direction *= osg::sign( force ) ;


    force = osg::absolute( force ) ;


    osg::Vec3   force_min( -FLT_MAX, -FLT_MAX, -FLT_MAX ) ;
    osg::Vec3   force_max( FLT_MAX, FLT_MAX, FLT_MAX ) ;

    if( m_servo_limit_x ) {
        force_min.x() = m_servo_force_min.x() ;
        force_max.x() = m_servo_force_max.x() ;
    }

    if( m_servo_limit_y ) {
        force_min.y() = m_servo_force_min.y() ;
        force_max.y() = m_servo_force_max.y() ;
    }

    if( m_servo_limit_z ) {
        force_min.z() = m_servo_force_min.z() ;
        force_max.z() = m_servo_force_max.z() ;
    }


    osg::Vec3   force3 = direction * force ;
    bool        force_local = false ;


    if( m_linear_velocity_local ) {

        force_local = true ;

        force3 = body->getQuaternion().inverse() * force3 ;
    }


    force3.x() = osg::clampTo( force3.x(), force_min.x(), force_max.x() ) ;
    force3.y() = osg::clampTo( force3.y(), force_min.y(), force_max.y() ) ;
    force3.z() = osg::clampTo( force3.z(), force_min.z(), force_max.z() ) ;




    body->addForce( force3, osg::Vec3(), force_local ) ;
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
#define IS_ZERO(VEC3_VALUE) ( (VEC3_VALUE).length() < 1.0e-3 )
/* ....................................................................... */
void
MotionActuator::doSimpleMotion( osgODE::RigidBody* body )
{
    const osg::Quat quat = body->getQuaternion() ;

    const osg::Vec3 translation         = m_translation_local       ?   ( quat * m_translation )        : m_translation ;
    const osg::Vec3 rotation            = m_rotation_local          ?   ( quat * m_rotation )           : m_rotation ;
    const osg::Vec3 linear_velocity     = m_linear_velocity_local   ?   ( quat * m_linear_velocity )    : m_linear_velocity ;
    const osg::Vec3 angular_velocity    = m_angular_velocity_local  ?   ( quat * m_angular_velocity )   : m_angular_velocity ;


    if( ! IS_ZERO( translation ) ) {
        body->setPosition( body->getPosition() + translation ) ;
    }



    if( ! IS_ZERO( rotation ) ) {

        osg::Quat   offset( rotation.x(),   osg::X_AXIS,
                            rotation.y(),   osg::Y_AXIS,
                            rotation.z(),   osg::Z_AXIS ) ;

        body->setQuaternion( body->getQuaternion() * offset ) ;
    }




    // do not increase the accumulators if the body is disabled
    if( body->getBodyEnabled() ) {


        if( ! IS_ZERO( m_force ) ) {

            body->addForce( m_force, osg::Vec3(), m_force_local, true ) ;
        }



        if( ! IS_ZERO( m_torque ) ) {

            body->addTorque( m_torque, m_torque_local ) ;
        }
    }



    if( ! IS_ZERO( linear_velocity ) ) {

        if( m_linear_velocity_additive ) {
            body->setLinearVelocity( body->getLinearVelocity() + m_linear_velocity ) ;
        } else {
            body->setLinearVelocity( m_linear_velocity ) ;
        }
    }



    if( ! IS_ZERO( angular_velocity ) ) {

        body->setAngularVelocity( m_angular_velocity ) ;
    }
}
/* ....................................................................... */
/* ======================================================================= */
